#!/usr/bin/env python3
# coding:utf-8

import time
import math
from dataclasses import dataclass, field
from typing import List

import rclpy
from rclpy.node import Node
from std_msgs.msg import String, Float64, Float64MultiArray, Int64, UInt64MultiArray


@dataclass
class sub_topic_hz:
    name: str
    sub: None
    fixed_hz: int  # 固有频率
    fixed_hz_diff_time: float  # 固定帧时间差

    # 记录时间, 数据
    hz: float = 0.0
    min_time: float = 1000.0
    max_time: float = 0.0
    new_start_time: float = 0.0  # 上一秒开始统计时间
    last_time: float = 0.0  # 上一条topic话题时间
    times: List[float] = field(default_factory=list)
    # 统计时间
    accum_dev_time: float = 0.0  # 积累偏差
    avg_dev_time: float = 0.0  # 积累偏差平均值


# import debugpy
# debugpy.listen(6688)
# debugpy.wait_for_client()


class topicSubscribeHz(Node):

    def __init__(self):
        super().__init__("topic_subscribe_count")
        self.sub_string = self.create_subscription(
            String, "/hz_string", self.callback_hz_string, 1
        )
        self.sub_float = self.create_subscription(
            Float64, "/hz_float", self.callback_hz_float, 1
        )
        self.sub_floatArray = self.create_subscription(
            Float64MultiArray, "/hz_float_array", self.callback_hz_floatArray, 1
        )
        self.sub_int = self.create_subscription(
            Int64, "/hz_int", self.callback_hz_int, 1
        )
        self.sub_intArray = self.create_subscription(
            UInt64MultiArray, "/hz_int_array", self.callback_hz_intArray, 1
        )
        self.data_init()

    def data_init(self):
        self.hz_string = sub_topic_hz("/hz_string", self.sub_string, 100, 1.0 / 100)
        self.hz_int = sub_topic_hz("/hz_int", self.sub_int, 100, 1.0 / 100)
        self.hz_float = sub_topic_hz("/hz_float", self.sub_float, 100, 1.0 / 100)
        self.hz_floatArray = sub_topic_hz(
            "/hz_float_array", self.sub_floatArray, 50, 1.0 / 50
        )
        self.hz_intArray = sub_topic_hz(
            "/hz_int_array", self.sub_intArray, 50, 1.0 / 50
        )

    def cal_hz(self, hz):
        cur_time = time.time()
        diff_time = cur_time - hz.new_start_time
        if hz.new_start_time == 0.0:
            hz.new_start_time = cur_time
        elif diff_time >= 1.0:
            if hz.hz != 0.0:
                hz.accum_dev_time += hz.fixed_hz * hz.fixed_hz_diff_time
                hz.min_time = min(hz.times)
                hz.max_time = max(hz.times)
                mean = sum(hz.times) / hz.hz
                rate = 1.0 / mean if mean > 0.0 else 0
                std_dev = math.sqrt(sum((x - mean) ** 2 for x in hz.times) / hz.hz)
                print(
                    f"{hz.name:<10} hz:{hz.hz} rate:{rate:.2f} min:{hz.min_time:.3f} max:{hz.max_time:.3f} avg:{(1.0/hz.hz):.3f} accum_dev:{hz.accum_dev_time:.3f} std dev:{std_dev:.3f}"
                )
            hz.new_start_time = cur_time
            hz.hz = 0
            hz.min_time = 0.0
            hz.max_time = 0.0
            hz.avg_time = 0.0
            hz.accum_dev_time = 0.0
            hz.times = []
        else:
            hz.hz += 1
            hz.accum_dev_time += abs(hz.fixed_hz_diff_time * hz.hz - diff_time)
            diff_time = cur_time - hz.last_time
            hz.times += [diff_time]

        hz.last_time = cur_time

    def callback_hz_string(self, msg):
        hz = self.hz_string
        self.cal_hz(hz)

    def callback_hz_float(self, msg):
        hz = self.hz_float
        self.cal_hz(hz)

    def callback_hz_floatArray(self, msg):
        hz = self.hz_floatArray
        self.cal_hz(hz)

    def callback_hz_int(self, msg):
        hz = self.hz_int
        self.cal_hz(hz)

    def callback_hz_intArray(self, msg):
        hz = self.hz_intArray
        self.cal_hz(hz)


def main(args=None):
    rclpy.init(args=args)
    sub_hz = topicSubscribeHz()
    rclpy.spin(sub_hz)
    minimal_subscriber.destroy_node()
    rclpy.shutdown()


if __name__ == "__main__":
    main()
